#ifndef MATRIX4_HEADER_VIZMF_PROJECT
#define MATRIX4_HEADER_VIZMF_PROJECT

#include "vector3.h"
#include "vector4.h"

template < typename T>
class Matrix4T      // col major matrix
{
public:
  typedef T float_type;
  typedef unsigned int index_type;
  typedef Matrix4T<T> Self;

  Matrix4T() {}
  template < typename T1>
  Matrix4T(T1 data[16])
  {
    for (index_type i = 0; i < 16; ++i) data_[i] = data[i];
  }
  template <typename T1>
  Matrix4T(T1** data)
  {
    for (index_type i = 0; i < 4; ++i)
      for (index_type j = 0; j < 4; ++j)
        data_[j*4+i] = data[i][j];
  }
  template <typename T1>
  Matrix4T(const Vector3T<T1>& x, const Vector3T<T1>& y, const Vector3T<T1>& z, const Vector3T<T1>& s = Vector3T<T1>(1.0, 1.0, 1.0), const Vector3T<T1>& t = Vector3T<T1>(0.0, 0.0, 0.0) )
  {
    // creates transformation matrix
    data_[0] = x[0]*s[0]; data_[4] = y[0]*s[1]; data_[8] = z[0]*s[2]; data_[12] = t[0];
    data_[1] = x[1]*s[0]; data_[5] = y[1]*s[1]; data_[9] = z[1]*s[2]; data_[13] = t[1];
    data_[2] = x[2]*s[0]; data_[6] = y[2]*s[1]; data_[10] = z[2]*s[2]; data_[14] = t[2];
    data_[3] = 0; data_[7] = 0; data_[11] = 0; data_[15] = 1.0;
  }
  Matrix4T(float_type fov, float_type aspect, float_type near, float_type far)
  {
    // creates perspective projection matrix
    // fov - horizontal fov angle in degrees
    // aspect = width/height
    float xmin, xmax, ymin, ymax; // for near clipping plane

    ymax = near * float(tan( fov/aspect * 3.14159265358979323 / 360.0 ));
    ymin = -ymax;
    xmin = ymin * aspect;
    xmax = -xmin;

   /* xmax = tan(fov/2.0/180.0*3.14159265358979323)*near;
    xmin = -xmax;
    ymin = xmin * aspect;
    ymax = -ymin; */
    data_[0] = (2.0 * near)/(xmax - xmin);
    data_[1] = data_[2] = data_[3] = data_[4] = 0.0;
    data_[5] = (2.0 * near)/(ymax - ymin);
    data_[6] = data_[7] = 0.0;
    data_[8] = (xmax + xmin) / (xmax - xmin);
    data_[9] = (ymax + ymin) / (ymax - ymin);
    data_[10] = -((far + near)/(far - near));
    data_[11] = -1.0;
    data_[12] = data_[13] = 0.0;
    data_[14] = -((2.0 * far * near)/(far - near));
    data_[15] = 0.0;
  }
  void loadIdentity() { for (index_type i = 0 ; i < 4; ++i) for (index_type j = 0; j < 4; ++j) data_[i*4 +j] = (i == j) ? 1 : 0; }
  void loadZero() { for (index_type i = 0; i < 16; ++i) data_[i] = 0.0; }
  const float_type operator () (const index_type i, const index_type j) const {return data_[j*4+i]; }
  float_type& operator () (const index_type i, const index_type j) {return data_[j*4+i]; }
  template <typename T1>
  Vector3T<T1> operator * (const Vector3T<T1>& vec) const
  {
    return Vector3T<T1>(data_[0] * vec[0] + data_[4] * vec[1] + data_[8] * vec[2] + data_[12],
                        data_[1] * vec[0] + data_[5] * vec[1] + data_[9] * vec[2] + data_[13],
                        data_[2] * vec[0] + data_[6] * vec[1] + data_[10] * vec[2] + data_[14]);
  }
  template <typename T1>
  Vector4T<T1> operator * (const Vector4T<T1>& vec) const
  {
    return Vector4T<T1>(data_[0] * vec[0] + data_[4] * vec[1] + data_[8] * vec[2] + data_[12] * vec[3],
                   data_[1] * vec[0] + data_[5] * vec[1] + data_[9] * vec[2] + data_[13] * vec[3],
                   data_[2] * vec[0] + data_[6] * vec[1] + data_[10] * vec[2] + data_[14] * vec[3],
                   data_[3] * vec[0] + data_[7] * vec[1] + data_[11] * vec[2] + data_[15] * vec[3]);
  }
  Self operator * (const Self& mat) const
  {
    Self a;
    a.loadZero();
    for (index_type i = 0 ; i < 4; ++i) 
      for (index_type j = 0 ; j < 4; ++j)
        for (index_type k = 0 ; k < 4; ++k)
          a.data_[j*4+i] += data_[k*4 + i] * mat.data_[j*4 + k];
    return a;
  }
  const Self& operator *= (const Self& mat)
  {
     return *this = mat * *this;
  }
  const float_type* data() const { return data_; }

  // some comvenience functionce follow
  template <typename T1>
  void rotate(const Vector3T<T1>& axis = Vector3T<T1>(1.0, 0.0, 0.0), const float_type angle = 0.0)
  {
    Matrix4T rot;
    Vector3T<T1> a = axis; a.normalize();
    Vector3T<T1> b = Vector3T<T1>(a[0]*a[0], a[1]*a[1], a[2]*a[2]);
    float_type c = cos(angle), s = sin(angle);
    rot(0,0) = c + (1.0 - c) * b[0];
    rot(0,1) = a[0]*a[1] * (1.0 - c) - a[2]*s;
    rot(0,2) = a[0]*a[2] * (1.0 - c) + a[1]*s;
    rot(0,3) = 0.0;
    rot(1,0) = a[0]*a[1] * (1.0 - c) + a[2]*s;
    rot(1,1) = c + (1.0 - c)*b[1];
    rot(1,2) = a[1]*a[2]*(1.0 - c) - a[0]*s;
    rot(1,3) = 0.0;
    rot(2,0) = a[0]*a[2]*(1.0 - c) - a[1] * s;
    rot(2,1) = a[1]*a[2]*(1.0 - c) + a[0] * s;
    rot(2,2) = c + (1.0 - c)*b[2];
    rot(2,3) = 0.0;
    rot(3,0) = 0.0;
    rot(3,1) = 0.0;
    rot(3,2) = 0.0;
    rot(3,3) = 1.0;
    *this *= rot ;
  }
  template < typename T1>
  void scale(const Vector3T<T1>& vec)
  {
    Self sc;
    sc(0,0) = vec[0];
    sc(0,1) = 0;
    sc(0,2) = 0;
    sc(0,3) = 0;
    sc(1,0) = 0;
    sc(1,1) = vec[1];
    sc(1,2) = 0;
    sc(1,3) = 0;
    sc(2,0) = 0;
    sc(2,1) = 0;
    sc(2,2) = vec[2];
    sc(2,3) = 0;
    sc(3,0) = 0;
    sc(3,1) = 0;
    sc(3,2) = 0;
    sc(3,3) = 1;
    *this *= sc;
  }
  template <typename T1>
  void translate(const Vector3T<T1>& vec)
  {
    Self mat;
    mat.loadIdentity();
    mat(0,3) = vec[0];
    mat(1,3) = vec[1];
    mat(2,3) = vec[2];
    *this *= mat;
  }
  Self toNormalMatrix() const
  {
    Self result;
    Vector3 v;
    v = Vector3(data_[0], data_[1], data_[2]);
    v.normalize();
    result(0,0) = v[0]; result(1,0) = v[1]; result(2,0) = v[2]; result(3,0) = 0.0;
    
    v = Vector3(data_[4], data_[5], data_[6]);
    v.normalize();
    result(0,1) = v[0]; result(1,1) = v[1]; result(2,1) = v[2]; result(3,1) = 0.0;
    
    v = Vector3(data_[8], data_[9], data_[10]);
    v.normalize();
    result(0,2) = v[0]; result(1,2) = v[1]; result(2,2) = v[2]; result(3,2) = 0.0;
    
    result(0,3) = 0.0; result(1,3) = 0.0; result(2,3) = 0.0; result(3,3) = 1.0;
    
    return result;
  }

private:
  float_type data_[16];
};

typedef Matrix4T<float> Matrix4f;
typedef Matrix4T<double> Matrix4d;
typedef Matrix4f Matrix4;

template <typename T1, typename T2>
inline Vector3T<T1> operator * (const Vector3T<T1>& vec, const Matrix4T<T2>& mat)
{
  return Vector3T<T1>(mat(0,0) * vec[0] + mat(0,1) * vec[1] + mat(0,2) * vec[2] + mat(0,3),
                      mat(1,0) * vec[0] + mat(1,1) * vec[1] + mat(1,2) * vec[2] + mat(1,3),
                      mat(2,0) * vec[0] + mat(2,1) * vec[1] + mat(2,2) * vec[2] + mat(2,3));
}

#ifdef DEBUG
template <typename T>
inline std::ostream& operator << (std::ostream& strm, const Matrix4T<T>& mat)
{
  return strm << mat(0,0) << " " << mat(0,1) << " " << mat(0,2) << " " << mat(0,3) << std::endl
       << mat(1,0) << " " << mat(1,1) << " " << mat(1,2) << " " << mat(1,3) << std::endl
       << mat(2,0) << " " << mat(2,1) << " " << mat(2,2) << " " << mat(2,3) << std::endl
       << mat(3,0) << " " << mat(3,1) << " " << mat(3,2) << " " << mat(3,3) << std::endl;

}
#endif

#endif // MATRIX4_HEADER_VIZMF_PROJECT
